﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

// PhysicalRigSupport

[Serializable]
public class Chain
{
    public float stiffness = 0.5f;
    public float mass = 1;
    public List<Vector3> spring;
    public List<Vector3> dir;
    public List<float> springForce;
    public float chainLength;
    public List<float> len; // 当前长度
    public List<float> lenRest; // 原本的长度
    public Vector3[] childPosRest; // 子骨骼的位置
    public float lenFactor = 1; //
    public List<float> lenDiff;
    public List<Edge> edges;
    public Transform[] verts;
    public bool[] pins;
    public int space;
    public int verticeCount { get { return verts.Length; } }
    public Vector3[] forceResultant;
    public Vector3[] vertsPosPre;
    public bool debugDrawDir = false;
    public bool debugDrawOneWay = false;
    public float oneWayBoneLength;

    public void InitEdgeSetting(List<Transform> vertice)
    {
        InitEdgeSetting(vertice.ToArray());
    }
    public void InitEdgeSetting(Transform[] verts)
    {
        this.verts = verts;
        vertsPosPre = new Vector3[verts.Length];
        childPosRest = new Vector3[verts.Length - 1];
        for (int i = 0; i < verts.Length - 1; i++)
        {
            childPosRest[i] = verts[i].InverseTransformPoint(verts[i + 1].position); // 骨骼i+1 相对于 骨骼i 的位置
        }
        for (int i = 0; i < verts.Length; i++)
        {
            vertsPosPre[i] = verts[i].position;
        }
        pins = new bool[verts.Length];
        lenRest = new List<float>();
        edges = new List<Edge>();
        for (int i = 0; i < verts.Length - space; i++)
        {
            edges.Add(new Edge() { V1 = verts[i], V2 = verts[i + space] }); // nV = nBx + space 
        }
        foreach (var edge in edges)
        {
            var dist = Vector3.Distance(edge.V1.position, edge.V2.position) * lenFactor;
            lenRest.Add(dist);
        }
        // 累计总长度
        var lenChainRest = 0f;
        foreach (var lenRest in lenRest)
        {
            lenChainRest += lenRest;
        }
    }
    internal void UpdateSpring(float stiffnessFactor)
    {
        len = new List<float>();
        lenDiff = new List<float>();
        dir = new List<Vector3>();
        springForce = new List<float>(); // nBx * 1
        int e = 0;
        foreach (var edge in edges)
        {
            len.Add(Vector3.Distance(edge.V1.position, edge.V2.position));
            lenDiff.Add(len[e] - lenRest[e]);
            // 长度还原力
            springForce.Add(stiffnessFactor * stiffness * lenDiff[e]);
            dir.Add((edge.V2.position - edge.V1.position).normalized);
            if (!pins[e]) forceResultant[e] += springForce[e] * dir[e] / mass; // v1
            if (!pins[e + space]) forceResultant[e + space] += +springForce[e] * -dir[e] / mass; // v2
            e++;
        }
    }

    public void FixLookAt()
    {
        var poss = new Vector3[verts.Length];
        for (int i = 0; i < verts.Length; i++) // 保存位置
        {
            poss[i] = verts[i].position;
        }
        for (int i = 0; i < verts.Length - 1; i++) // 更改朝向
        {
            verts[i].up = (verts[i + 1].position - verts[i].position).normalized;
            //verts[i].right = -(verts[i + 1].position - verts[i].position).normalized;
            //verts[i].forward = -(verts[i + 1].position - verts[i].position).normalized;
        }
        for (int i = 0; i < verts.Length; i++) // 还原位置。如果父子层级关系是比较复杂的话有出错可能
        {
            verts[i].position = poss[i];
        }
    }

    public void OneWaySpring(float stiffnessFactor)
    {
        for (int i = 0; i < verts.Length - 1; i++)
        {
            var restPos = verts[i].TransformPoint(childPosRest[i]);
            var diff = restPos - verts[i + 1].position;
            float force = diff.magnitude * stiffnessFactor;
            if (debugDrawDir) Debug.DrawRay(verts[i + 1].position, Vector3.up * force, Color.blue, 0, false);

            var dir = (restPos - verts[i + 1].position).normalized;
            if (debugDrawDir) Debug.DrawRay(verts[i + 1].position, dir * force, Color.red, 0, false);

            if (!pins[i + 1]) forceResultant[i + 1] += force * dir / mass;
        }
    }
    internal void UpdateSpring(List<Vector3> listForceRe, float stiffnessFactor)
    {
        if (listForceRe.Count != verticeCount)
            throw new Exception("合力的数量不匹配。");

        len = new List<float>();
        lenDiff = new List<float>();
        dir = new List<Vector3>();
        springForce = new List<float>(); // nBx * 1
        int e = 0;
        foreach (var edge in edges)
        {
            len.Add(Vector3.Distance(edge.V1.position, edge.V2.position));
            lenDiff.Add(len[e] - lenRest[e]);
            // 长度还原力
            springForce.Add(stiffnessFactor * stiffness * lenDiff[e]);
            dir.Add((edge.V2.position - edge.V1.position).normalized);

            listForceRe[e] += springForce[e] * dir[e] / mass; // v1
            listForceRe[e + space] += springForce[e] * -dir[e] / mass; // v2            
            e++;
        }
    }
    // 阻力
    public void AddDragForce(float dragFactor)
    {
        for (int i = 0; i < verts.Length; i++)
        {
            forceResultant[i] += dragFactor * (vertsPosPre[i] - verts[i].position);
        }
    }
    public void OneWayResetLength()
    {
        for (int i = 0; i < verts.Length - 1; i++)
        {
            var dir = (verts[i + 1].position - verts[i].position).normalized;
            verts[i + 1].position = verts[i].position + dir * lenRest[i];
        }
    }
    public void ClearForce()
    {
        forceResultant = new Vector3[verts.Length];
    }
    public void UpdateForceToVerts(float factor)
    {
        for (int i = 0; i < verts.Length; i++)
        {
            if (!pins[i])
            {
                verts[i].position = verts[i].position + forceResultant[i] * factor;
            }
            vertsPosPre[i] = verts[i].position;
        }
        for (int i = 0; i < verts.Length; i++)
        {
            if (debugDrawOneWay) Debug.DrawRay(verts[i].position, verts[i].up * oneWayBoneLength, Color.green, 0, false);
        }
    }
    internal void AddGobalForce(Vector3 force)
    {
        for (int i = 0; i < forceResultant.Length - 1; i++)
        {
            forceResultant[i] += force;
        }
    }
}
[Serializable]
public struct Edge // 一条边的两个点对象
{
    public Transform V1; // Vertex1
    public Transform V2;
}